//
// Created by xiang on 2022/7/18.
//

#include <gflags/gflags.h>
#include <glog/logging.h>

#include "ch7/loam-like/loam_like_odom.h"
#include "common/io_utils.h"
#include "common/timer/timer.h"
#include "tools/ui/pangolin_window.h"

DEFINE_string(file_path, "/home/zl/Public/kaist_data/urban_data/", "数据文件路径");
DEFINE_string(txt_path, "/home/zl/Public/kaist_data/urban_data/sensor_data/data_stamp.csv", "数据文件路径");
DEFINE_bool(display_map, false, "display map?");

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    ui::PangolinWindow ui;
    if (ui.Init() == false) {
        return -1;
    }
    // 测试loam-like odometry的表现
    sad::LoamLikeOdom::Options options;
    options.display_realtime_cloud_ = FLAGS_display_map;
    sad::LoamLikeOdom lo(options);

    sad::KaistCsvIO io(FLAGS_file_path, FLAGS_txt_path);
    io.SetPointCloud3DProcessFunc([&](const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_ptr) -> bool  {
            common::Timer::Evaluate(
                [&]() {
                    lo.ProcessPointCloud(cloud_ptr);
                },
                "Loam-like odom");

            if(ui.ShouldQuit() == false) {
                if(lo.GetLocalMap() != nullptr &&
                   lo.GetGlobalMap() != nullptr) {
                    if(options.display_realtime_cloud_) {
                        ui.UpdateScan(lo.GetLocalMap(), SE3());                        
                    } else {
                        ui.UpdateScan(lo.GetGlobalMap(), SE3());
                    }
                }               
            }

            return true;
    })
    .Go();


    lo.SaveMap("./data/ch7/loam_map.pcd");

    common::Timer::PrintAll();
    LOG(INFO) << "done.";

    return 0;
}
